/*
  SUMo IMU ROS driver node
  Author: SUMo
  Version: March 16
*/

#include "dual_node.h"
#include <iostream>
#include <fstream>

// Function prototypes go here:
//callback that will run if the Spatial is attached to the computer
int CCONV IMU_AttachHandler(CPhidgetHandle spatial, void *userptr);
int CCONV ENC_AttachHandler(CPhidgetHandle encoder, void *userptr);

//callback that will run if the Spatial is detached from the computer
int CCONV DetachHandler(CPhidgetHandle handle, void *userptr);

//callback that will run if the Spatial generates an error
int CCONV ErrorHandler(CPhidgetHandle handle, void *userptr, int ErrorCode, const char *unknown);

//callback that will run when a new datum arrives
int CCONV SpatialDataHandler(CPhidgetSpatialHandle spatial, void *userptr, 
			     CPhidgetSpatial_SpatialEventDataHandle *data, int count);

void PhidgetSensorNode::
setEncoderCovariance(double value) {
  for(int i = 0; i < 6; i++)
    enc_odom.pose.covariance[i*6+i] = value;

  for(int i = 0; i < 6; i++)
    enc_odom.twist.covariance[6*i+i] = 9999; // perra value;
}


PhidgetSensorNode::
PhidgetSensorNode(ros::NodeHandle n, int sample_period_msec)
  :imu_handle(0),enc_handle(0),nh(n),
#ifdef USE_MAHONY
   ahrs(sample_period_msec*0.001, MAHONY_P, MAHONY_I)//convert to sec
#else
   ahrs(sample_period_msec*0.001, 0.1)
#endif
{
  //Three stages initialization:
  // 1. Open the phidget module but does not start it
  // 2. Configure the ros publisher and message
  // 3. Configure the phidget module and start running
 
  init_ros_imu();
  init_phidget_enc();
  init_phidget_imu();
  //2. configure 
  ROS_INFO("eee");
  init_ros_enc();
  ROS_INFO("aa");
  //init_ros_imu();
  //3.config encoder first, IMU needs 4 sec of start up
  config_enc();
  config_imu(sample_period_msec);
}

// Open up the phidget module
void PhidgetSensorNode::
init_phidget_imu(void){
    //create the spatial object
    CPhidgetSpatial_create(&imu_handle);

    //Set the handlers to be run when the device is:
    //-> plugged in or opened from software, 
    //-> unplugged or closed from software,
    //-> generates an error.
    CPhidget_set_OnAttach_Handler((CPhidgetHandle)imu_handle, IMU_AttachHandler, NULL);
    CPhidget_set_OnDetach_Handler((CPhidgetHandle)imu_handle, DetachHandler, NULL);
    CPhidget_set_OnError_Handler((CPhidgetHandle)imu_handle,  ErrorHandler, NULL);

    //Registers a callback that will run according to the set data rate that will return the spatial data changes
    //Requires the handle for the Spatial, the callback handler function that will be called, 
    //and an arbitrary pointer that will be supplied to the callback function (may be NULL)
    CPhidgetSpatial_set_OnSpatialData_Handler(imu_handle, SpatialDataHandler, this);

    //open the spatial object for device connections
    CPhidget_open((CPhidgetHandle)imu_handle, -1);

    ROS_INFO("Waiting for spatial to be attached....");

    int result = CPhidget_waitForAttachment((CPhidgetHandle)imu_handle, 10000);
    if (result)
      {
	const char* err;
	CPhidget_getErrorDescription(result, &err);
	ROS_ERROR("Problem waiting for imu attachment: %s", err);
      }
    ROS_INFO("ddd");
}

void PhidgetSensorNode::
init_phidget_enc(void){
   //create the spatial object
    CPhidgetEncoder_create(&enc_handle);

    //Set the handlers to be run when the device is:
    //-> plugged in or opened from software, 
    //-> unplugged or closed from software,
    //-> generates an error.
    CPhidget_set_OnAttach_Handler((CPhidgetHandle)enc_handle, ENC_AttachHandler, NULL);
    CPhidget_set_OnDetach_Handler((CPhidgetHandle)enc_handle, DetachHandler, NULL);
    CPhidget_set_OnError_Handler((CPhidgetHandle)enc_handle,  ErrorHandler, NULL);

    //open the spatial object for device connections
    CPhidget_open((CPhidgetHandle)enc_handle, -1);

    ROS_INFO("Waiting for encoder to be attached....");

    int result = CPhidget_waitForAttachment((CPhidgetHandle)enc_handle, 10000);
    if(result)
      {
	const char* err;
	CPhidget_getErrorDescription(result, &err);
	ROS_ERROR("Problem waiting for enc attachment: %s", err);
      }
}

// Configure ROS related things then
void PhidgetSensorNode::
init_ros_imu(void){
  std::string frameid_;
  // Configure ROS publisher
  ROS_INFO("bbb");

  imu_pub = nh.advertise<sensor_msgs::Imu>("imu_data",50);

  nh.param("IMU_frameID", frameid_, std::string("base_footprint"));
  imu_pose.header.frame_id = frameid_;
  
  double linear_acceleration_stdev_, orientation_stdev_, angular_velocity_stdev_;
  // Get some parameters from the launch file at run time
  nh.param("linear_acceleration_stdev", linear_acceleration_stdev_, 0.098); 
  nh.param("orientation_stdev", orientation_stdev_, 0.035); 
  nh.param("angular_velocity_stdev", angular_velocity_stdev_, 0.012); 
  
  // Set the covariance matrices
  double angular_velocity_covariance = angular_velocity_stdev_ * angular_velocity_stdev_;
  double orientation_covariance = orientation_stdev_ * orientation_stdev_;
  double linear_acceleration_covariance = linear_acceleration_stdev_ * linear_acceleration_stdev_;
  
  imu_pose.linear_acceleration_covariance[0] = linear_acceleration_covariance;
  imu_pose.linear_acceleration_covariance[4] = linear_acceleration_covariance;
  imu_pose.linear_acceleration_covariance[8] = linear_acceleration_covariance;
  
  imu_pose.angular_velocity_covariance[0] = angular_velocity_covariance;
  imu_pose.angular_velocity_covariance[4] = angular_velocity_covariance;
  imu_pose.angular_velocity_covariance[8] = angular_velocity_covariance;
  
  imu_pose.orientation_covariance[0] = orientation_covariance;
  imu_pose.orientation_covariance[4] = orientation_covariance;
  imu_pose.orientation_covariance[8] = orientation_covariance;
}

void PhidgetSensorNode::
init_ros_enc(void){
  //Configure ROS publisher
  odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 50);

  std::string frameid_;
  nh.param("ENC_frameID", frameid_, std::string("robot_base"));
  
  enc_odom.header.frame_id = frameid_;

  // Initialize position
  enc_odom.pose.pose.position.x = 0.0;
  enc_odom.pose.pose.position.y = 0.0;
  enc_odom.pose.pose.position.z = 0.0;

  // Orientation
  enc_odom.pose.pose.orientation.x = 0.0;
  enc_odom.pose.pose.orientation.y = 0.0;
  enc_odom.pose.pose.orientation.z = 0.0;
  enc_odom.pose.pose.orientation.w = 1.0;

  // Linear velocities
  enc_odom.twist.twist.linear.x = 0.0;
  enc_odom.twist.twist.linear.y = 0.0;
  enc_odom.twist.twist.linear.z = 0.0;
  // Angular velocities
  enc_odom.twist.twist.angular.x = 0.0;
  enc_odom.twist.twist.angular.y = 0.0;
  enc_odom.twist.twist.angular.z = 0.0;

  // Set the covariances
  setEncoderCovariance(0.1);

  // Zero them
  CPhidgetEncoder_setPosition(enc_handle, LF, 0);
  //CPhidgetEncoder_setPosition(enc_handle, LB, 0);
  CPhidgetEncoder_setPosition(enc_handle, RF, 0);
  //  CPhidgetEncoder_setPosition(enc_handle, RB, 0);

}

// Configure the phidget module
void PhidgetSensorNode::
config_imu(int sample_period_msec){
  // Open up a file to load the magnetic calibration parameters
  std::string config_file;
  nh.param("config_file", config_file, std::string("imu_calib.txt"));
  
  FILE* fd = fopen(config_file.c_str(), "r");
  if (fd != NULL)
    {
      int count;
      double magF, ofs0, ofs1, ofs2, g0, g1, g2, t0, t1, t2, t3, t4, t5;
      count=fscanf(fd, "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
		   &magF, &ofs0, &ofs1, &ofs2, &g0, &g1, &g2,
		   &t0, &t1, &t2, &t3, &t4, &t5);
      if (count != 13)
	ROS_ERROR("IMU:compass parameter wrong number");
      
      //Set compass parameters
#define SET_COMPASS CPhidgetSpatial_setCompassCorrectionParameters
      SET_COMPASS(imu_handle,magF, ofs0, ofs1, ofs2, g0, g1, g2,
		  t0, t1, t2, t3, t4, t5);
      fclose(fd);
    }
  else
    {
      ROS_ERROR("IMU:cannot load compass param file, continue with default values");
      SET_COMPASS(imu_handle,0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
		    0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
      }

  //Set the data rate for the spatial events
  //and start streaming
  CPhidgetSpatial_setDataRate(imu_handle, sample_period_msec);
}

void PhidgetSensorNode::
config_enc()
{}

PhidgetSensorNode::
~PhidgetSensorNode(){

  CPhidget_close((CPhidgetHandle)imu_handle);
  CPhidget_close((CPhidgetHandle)enc_handle);
  CPhidget_delete((CPhidgetHandle)imu_handle);
  CPhidget_delete((CPhidgetHandle)enc_handle);
}
  
//IMU fusion algorithm, and publish the data
//Get called in the IMU event callback
void PhidgetSensorNode::
processImuData(CPhidgetSpatial_SpatialEventDataHandle sd){

  //ROS_INFO("Number of Data Packets in this event: %d\n", count);
  imu_pose.header.stamp = ros::Time::now();

#define gx   sd->acceleration[0]
#define gy ( sd->acceleration[1])
#define gz ( sd->acceleration[2])

#define mx ( sd->magneticField[0])
#define my ( sd->magneticField[1])
#define mz ( sd->magneticField[2])

#define ANGULAR imu_pose.angular_velocity  // Easier to read
  // Convert from deg/s t rad/s
  ANGULAR.x =  sd->angularRate[0] * 0.0174532925;
  ANGULAR.y =  sd->angularRate[1] * 0.0174532925;
  ANGULAR.z =  sd->angularRate[2] * 0.0174532925;
  
  // Mahony's DCM algorithm to compute Orientation in quaternion
#ifdef USE_COMPASS
  if(sd->magneticField[0] < 5000.0) // when the magneticfield reading is valid
    {
      ahrs.AHRSupdate(ANGULAR.x, ANGULAR.y, ANGULAR.z,
		      gx, gy, gz, mx, my, mz);
    }
  else
#endif
    {// then use only gyro and accelerometer
      ahrs.AHRSupdateIMU(ANGULAR.x, ANGULAR.y, ANGULAR.z,
			 gx, gy, gz);
    }
  
  imu_pose.linear_acceleration.x = gx;
  imu_pose.linear_acceleration.y = gy;
  imu_pose.linear_acceleration.z = gz;
  
  imu_pose.orientation.w = ahrs.q0;
  imu_pose.orientation.x = ahrs.q1;
  imu_pose.orientation.y = ahrs.q2;
  imu_pose.orientation.z = ahrs.q3;
  
  imu_pub.publish(imu_pose);
}

void PhidgetSensorNode::
processEncoderData(){
  static double angle;

  int lf, lb, rf, rb;//Left front, left back, right front, right back
 
  ros::Time oldtime = enc_odom.header.stamp;
  enc_odom.header.stamp = ros::Time::now();
  // Read data from encoders
  CPhidgetEncoder_getPosition(enc_handle, LF, &lf);
  //  CPhidgetEncoder_getPosition(enc_handle, LB, &lb);
  CPhidgetEncoder_getPosition(enc_handle, RF, &rf);
  //  CPhidgetEncoder_getPosition(enc_handle, RB, &rb);
  
  // Zero them
  CPhidgetEncoder_setPosition(enc_handle, LF, 0);
  //CPhidgetEncoder_setPosition(enc_handle, LB, 0);
  CPhidgetEncoder_setPosition(enc_handle, RF, 0);
  //  CPhidgetEncoder_setPosition(enc_handle, RB, 0);


  // Update pose
  double vl   = -(lf) * SCALER * 1.015;
  double vr   = (rf) * SCALER;
  double LinV = (vr + vl) * 0.5;
  double AngV = (vr - vl) * 0.5 / BASELINE;

  /* TODO(perra): uncomment when 4 encoders are working
  // Update pose
  double vl   = (lf + lb) * SCALER;
  double vr   = -(rf + rb)* SCALER;
  double LinV = (vr + vl) * 0.5;
  double AngV = (vr - vl) * 0.5 / BASELINE;
  */
  double delT = enc_odom.header.stamp.toSec() - oldtime.toSec();

  // We need to see whether or not we're turning and adjust the covariance
  // as necessary. When we turn, we want a high covariance.  When going straight,
  // we want a low covariance.
  
  double yaw = atan2(ahrs.q0*ahrs.q3+ahrs.q1*ahrs.q2, 0.5-ahrs.q2*ahrs.q2-ahrs.q3*ahrs.q3); 

  double vx = LinV * cos(yaw);
  double vy = LinV * sin(yaw);
  enc_odom.pose.pose.position.x += vx * delT;
  enc_odom.pose.pose.position.y += vy * delT;
  enc_odom.pose.pose.position.z += 0.0;

  enc_odom.pose.pose.orientation = imu_pose.orientation;
  
  // Linear velocities
  enc_odom.twist.twist.linear.x = vx;
  enc_odom.twist.twist.linear.y = vy;
  enc_odom.twist.twist.linear.z = 0.0;
  // Angular velocities
  enc_odom.twist.twist.angular.x = 0;
  enc_odom.twist.twist.angular.y = 0;
  enc_odom.twist.twist.angular.z = AngV;

  odom_pub.publish(enc_odom);

  if (false) {
      std::ofstream myfile;
      myfile.open("~/Project/ros_ws/dave/pose_ekf_simulation/output/raw_encoder.txt", std::ios::app);
      double secs = ros::Time::now().toSec();
      if (!myfile.is_open()) {
	ROS_ERROR("RAW ENCODER FILE NOT OPEN!");
      }
      myfile.precision(20);
      myfile << secs << " " 
	 << enc_odom.pose.pose.position.x << " "
	 << enc_odom.pose.pose.position.y << " "
	 << yaw << " "
	 << vl << " "
	 << vr << " "
         << LinV << " "
	 << AngV << " "
	 << "\n";
  
      myfile.close();
  }


}

//Main program
int main(int argc, char* argv[])
{
  // Initialize ROS
  ros::init(argc, argv, "imu_node");
  ros::NodeHandle n;

  PhidgetSensorNode imu(n, PERIOD_MSEC);//sample period of 20ms

  // must do this to enable the callbacks
  ros::spin();

  ROS_INFO("IMU node exit");
}

//callback that will run if the Spatial is attached to the computer
int CCONV IMU_AttachHandler(CPhidgetHandle spatial, void *userptr)
{
  int serialNo;
  CPhidget_getSerialNumber(spatial, &serialNo);
  ROS_INFO("Spatial %10d attached!", serialNo);
  
#ifdef ZERO_GYRO
  //Zero the gyro first
  //sleep(5);
  CPhidgetSpatial_zeroGyro((CPhidgetSpatialHandle) spatial);

  // Wait 4sec for the gyro stablize
  sleep(4);
  ROS_INFO("Gyro zero done, IMU ready to run");
#endif
  return 0;
}

int CCONV ENC_AttachHandler(CPhidgetHandle ENC, void *userptr){

  int serialNo;
  CPhidget_DeviceID deviceID;
  int i, inputcount;
  
  CPhidget_getSerialNumber(ENC, &serialNo);
  
  //Retrieve the device ID and number of encoders so that we can set the enables if needed
  CPhidget_getDeviceID(ENC, &deviceID);
  CPhidgetEncoder_getEncoderCount((CPhidgetEncoderHandle)ENC, &inputcount);
  ROS_INFO("Encoder %10d attached!", serialNo);
  
  //the 1047 requires enabling of the encoder inputs, so enable them if this is a 1047    
  if (deviceID == PHIDID_ENCODER_HS_4ENCODER_4INPUT)
    {
      ROS_INFO("Encoder requires Enable. Enabling inputs....");
      for (i = 0 ; i < inputcount ; i++)
	CPhidgetEncoder_setEnabled((CPhidgetEncoderHandle)ENC, i, 1);
    }
  return 0;
}

//callback that will run if the Spatial is detached from the computer
int CCONV DetachHandler(CPhidgetHandle spatial, void *userptr){

  int serialNo;
  CPhidget_getSerialNumber(spatial, &serialNo);
  ROS_INFO("Phidget %10d detached!", serialNo);
  
  return 0;
}

//callback that will run if the Spatial generates an error
int CCONV ErrorHandler(CPhidgetHandle handle, void *userptr, int ErrorCode, const char *unknown){

  ROS_ERROR("Error handled. %d - %s", ErrorCode, unknown);
  return 0;
}

// Period IMU event handler;
int CCONV SpatialDataHandler(CPhidgetSpatialHandle spatial, void *userptr, 
			     CPhidgetSpatial_SpatialEventDataHandle *data, int count){

  PhidgetSensorNode* ptr = ( PhidgetSensorNode* ) userptr;
  
  ptr->processImuData(*data);
  ptr->processEncoderData();
  return 0;
}
